/*
 * Gait.cpp
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */

#include <motion/Gait.h>
#include <math.h>

Gait::Gait(Robot* ST, int gaitnum, double setspeed) {
	rbe = ST;
	height = .32;
	width = .2;
	length = .21;
	duty = 0;
	offset0 = 0;
	offset1 = 0;
	offset2 = 0;
	offset3 = 0;
	offset4 = 0;
	Gait::setType(gaitnum);
	legmax = sqrt(pow(LINK1LENGTH + LINK2LENGTH + LINK3LENGTH, 2) - pow(length
			/ 2, 2)) - .1;
	speed = setspeed;
	speedwait = 1/(speed/length*offset4);
	walking = 1;
	gaitmotion = 1;
	logbool = 0;
}

Gait::~Gait() {
}

void Gait::calculateGait() {
	//cout << "#: " << gaitmotion << endl;
	//Leg 1 Motion Planning
	if ((gaitmotion > offset0) && (gaitmotion <= (offset0 + duty))) {
		//Where in the phase you are as you iterate through the gate (0 -> 360 degrees)
		double i0 = ((double) gaitmotion - (double) offset0) * (360.0
				/ (double) duty);
		rbe->legs[0].setDesiredx(rbe->legs[0].getDesiredx()
				+ (double)(rbe->getFl()) * width / (double) duty); // Calculates the desired x
		rbe->legs[0].setDesiredy(rbe->legs[0].getDesiredy()
				+ (double)(rbe->getVl()) * length / (double) duty);
		rbe->legs[0].setDesiredz(rbe->legs[0].getDesiredz()
				- (double)(rbe->getHl()) * sin(i0 * (double) (PI / 180.0))
						* 2.0 / (double) duty * height);
		if (walking == 1) {
			//weight on body = 0
		} else {
			//weight on body = -264
		}
	}
	if ((gaitmotion < offset0) || (gaitmotion >= (offset0 + duty))) {
		rbe->legs[0].setDesiredx(rbe->legs[0].getDesiredx()
				- (double) rbe->getFl() * width / ((double) offset4
						- (double) duty)); // Calculates the desired x
		rbe->legs[0].setDesiredy(rbe->legs[0].getDesiredy()
				- (double) rbe->getVl() * length / ((double) offset4
						- (double) duty));
		rbe->legs[0].setDesiredz(rbe->legs[0].getDesiredz());
		//weight on body = -264
	}

	//Leg 2 Motion Planning
	if ((gaitmotion > offset2) && (gaitmotion <= (offset2 + duty))) {
		//Where in the phase you are as you iterate through the gate (0 -> 360 degrees)
		double i2 = ((double) gaitmotion - (double) offset2) * (360.0
				/ (double) duty);
		rbe->legs[1].setDesiredx(rbe->legs[1].getDesiredx()
				+ (double) rbe->getFl() * width / (double) duty); // Calculates the desired x
		rbe->legs[1].setDesiredy(rbe->legs[1].getDesiredy()
				+ (double) rbe->getVl() * length / (double) duty);
		rbe->legs[1].setDesiredz(rbe->legs[1].getDesiredz()
				- (double) (rbe->getHl()) * sin(i2 * (double) (PI / 180.0))
						* 2.0 / (double) duty * height);
		if (walking == 1) {
			//weight on body = 0
		} else {
			//weight on body = -264
		}
	}
	if ((gaitmotion < offset2) || (gaitmotion >= (offset2 + duty))) {
		rbe->legs[1].setDesiredx(rbe->legs[1].getDesiredx()
				- (double) rbe->getFl() * width / ((double) offset4
						- (double) duty)); // Calculates the desired x
		rbe->legs[1].setDesiredy(rbe->legs[1].getDesiredy()
				- (double) rbe->getVl() * length / ((double) offset4
						- (double) duty));
		rbe->legs[1].setDesiredz(rbe->legs[1].getDesiredz());
		//weight on body = -264
	}

	//Leg 3 Motion Planning
	if ((gaitmotion > offset1) && (gaitmotion <= (offset1 + duty))) {
		//Where in the phase you are as you iterate through the gate (0 -> 360 degrees)
		double i1 = ((double) gaitmotion - (double) offset1) * (360.0
				/ (double) duty);
		//cout << "Angle: " << i1 <<  endl;
		rbe->legs[2].setDesiredx(rbe->legs[2].getDesiredx()
				+ (double) rbe->getBl() * width / (double) duty); // Calculates the desired x
		rbe->legs[2].setDesiredy(rbe->legs[2].getDesiredy()
				+ (double) rbe->getVl() * length / (double) duty);
		rbe->legs[2].setDesiredz(rbe->legs[2].getDesiredz()
				- (double) (rbe->getHl()) * sin(i1 * (double) (PI / 180.0))
						* 2.0 / (double) duty * height);
		if (walking == 1) {
			//weight on body = 0
		} else {
			//weight on body = -264
		}
	}
	if ((gaitmotion < offset1) || (gaitmotion >= (offset1 + duty))) {
		rbe->legs[2].setDesiredx(rbe->legs[2].getDesiredx()
				- (double) rbe->getBl() * width / ((double) offset4
						- (double) duty)); // Calculates the desired x
		rbe->legs[2].setDesiredy(rbe->legs[2].getDesiredy()
				- (double) rbe->getVl() * length / ((double) offset4
						- (double) duty));
		rbe->legs[2].setDesiredz(rbe->legs[2].getDesiredz());
		//weight on body = -264
	}

	//Leg 4 Motion Planning
	if ((gaitmotion > offset3) && (gaitmotion <= (offset3 + duty))) {
		//Where in the phase you are as you iterate through the gate (0 -> 360 degrees)
		double i3 = ((double) gaitmotion - (double) offset3) * (360.0
				/ (double) duty);
		rbe->legs[3].setDesiredx(rbe->legs[3].getDesiredx()
				+ (double) rbe->getBl() * width / (double) duty); // Calculates the desired x
		rbe->legs[3].setDesiredy(rbe->legs[3].getDesiredy()
				+ (double) rbe->getVl() * length / (double) duty);
		rbe->legs[3].setDesiredz(rbe->legs[3].getDesiredz()
				- (double) (rbe->getHl()) * sin(i3 * (double) (PI / 180.0))
						* 2.0 / (double) duty * height);
		if (walking == 1) {
			//weight on body = 0
		} else {
			//weight on body = -264
		}
	}
	if ((gaitmotion < offset3) || (gaitmotion >= (offset3 + duty))) {
		rbe->legs[3].setDesiredx(rbe->legs[3].getDesiredx()
				- (double) rbe->getBl() * width / ((double) offset4
						- (double) duty)); // Calculates the desired x
		rbe->legs[3].setDesiredy(rbe->legs[3].getDesiredy()
				- (double) rbe->getVl() * length / ((double) offset4
						- (double) duty));
		rbe->legs[3].setDesiredz(rbe->legs[3].getDesiredz());
		//weight on body = -264
	}
	//cout << "duty: " << duty << " offset: " << nona << " total: " << nonb << endl;

}

void Gait::printDesiredPositions() {
	cout << "Leg 0, X: " << rbe->legs[0].getDesiredx() << " Y: "
			<< rbe->legs[0].getDesiredy() << " Z: "
			<< rbe->legs[0].getDesiredz() << endl;
	cout << "Leg 1, X: " << rbe->legs[1].getDesiredx() << " Y: "
			<< rbe->legs[1].getDesiredy() << " Z: "
			<< rbe->legs[1].getDesiredz() << endl;
	cout << "Leg 2, X: " << rbe->legs[2].getDesiredx() << " Y: "
			<< rbe->legs[2].getDesiredy() << " Z: "
			<< rbe->legs[2].getDesiredz() << endl;
	cout << "Leg 3, X: " << rbe->legs[3].getDesiredx() << " Y: "
			<< rbe->legs[3].getDesiredy() << " Z: "
			<< rbe->legs[3].getDesiredz() << endl;
}

int Gait::getLogbool() const {
	return logbool;
}

void Gait::setLogbool(int logbool) {
	this->logbool = logbool;
}

void Gait::logDesiredPositions() {

	ofstream mylog;

	/*
	 int t2s=Gait::getType();
	 string s;
	 stringstream out;
	 out<<t2s<<"DesirePositionLog";
	 s=out.str();*/
	if (logbool == 0) {
		mylog.open("logDesiredPositions.txt", ios::out | ios::trunc);
		logbool = 1;
	} else {
		mylog.open("logDesiredPositions.txt", ios::out | ios::app);
	}

	mylog << rbe->legs[0].getDesiredx() << endl << rbe->legs[0].getDesiredy()
			<< endl << rbe->legs[0].getDesiredz() << endl
			<< rbe->legs[1].getDesiredx() << endl << rbe->legs[1].getDesiredy()
			<< endl << rbe->legs[1].getDesiredz() << endl
			<< rbe->legs[2].getDesiredx() << endl << rbe->legs[2].getDesiredy()
			<< endl << rbe->legs[2].getDesiredz() << endl
			<< rbe->legs[3].getDesiredx() << endl << rbe->legs[3].getDesiredy()
			<< endl << rbe->legs[3].getDesiredz() << endl;
	mylog.close();

}

int Gait::getDuty() const {
	return duty;
}

void Gait::printInfo() {
	cout << "Gait Height: " << Gait::getHeight() << endl;
	cout << "Gait Length: " << Gait::getLength() << endl;
	cout << "Gait Width: " << Gait::getWidth() << endl;
	cout << "Gait Type: " << Gait::getType() << endl;
	cout << "Gait Duty: " << Gait::getDuty() << endl;
	cout << "Gait Offset0: " << Gait::getOffset0() << endl;
	cout << "Gait Offset1: " << Gait::getOffset1() << endl;
	cout << "Gait Offset2: " << Gait::getOffset2() << endl;
	cout << "Gait Offset3: " << Gait::getOffset3() << endl;
	cout << "Gait Offset4: " << Gait::getOffset4() << endl;
	cout << "Gait Legmax: " << Gait::getLegmax() << endl;
	cout << "Gait Speed: " << Gait::getSpeed() << endl;
}

double Gait::getHeight() const {
	return height;
}

double Gait::getLegmax() const {
	return legmax;
}

double Gait::getLength() const {
	return length;
}

int Gait::getOffset0() const {
	return offset0;
}

int Gait::getOffset1() const {
	return offset1;
}

int Gait::getOffset2() const {
	return offset2;
}

int Gait::getOffset3() const {
	return offset3;
}

int Gait::getOffset4() const {
	return offset4;
}

int Gait::getType() const {
	return type;
}

bool Gait::getWalking() const {
	return walking;
}

double Gait::getWidth() const {
	return width;
}

void Gait::setDuty(int duty) {
	this->duty = duty;
}

void Gait::setHeight(double height) {
	this->height = height;
}

void Gait::setLegmax(double legmax) {
	this->legmax = legmax;
}

void Gait::setLength(double length) {
	this->length = length;
}

void Gait::setOffset0(int offset1) {
	this->offset0 = offset0;
}

void Gait::setOffset1(int offset1) {
	this->offset1 = offset1;
}

void Gait::setOffset2(int offset2) {
	this->offset2 = offset2;
}

void Gait::setOffset3(int offset3) {
	this->offset3 = offset3;
}

void Gait::setOffset4(int offset4) {
	this->offset4 = offset4;
}

void Gait::setType(int type) {
	this->type = type;
	switch (type) {
	//Lateral Sequence Walking
	case 1:
		duty = 30;
		offset0 = 0;
		offset1 = 25;
		offset2 = 50;
		offset3 = 75;
		offset4 = offset3 + duty;
		break;
		//Walking Trot
	case 2:
		duty = 40;
		offset0 = 10;
		offset1 = 10;
		offset2 = 60;
		offset3 = 60;
		offset4 = offset3 + duty;
		break;
		//Running Trot
	case 3:
		duty = 60;
		offset0 = 90;
		offset1 = 90;
		offset2 = 40;
		offset3 = 40;
		offset4 = offset0 + duty;
		break;
		//Rotary Gallop
	case 4:
		duty = 70;
		offset0 = 90;
		offset1 = 40;
		offset2 = 80;
		offset3 = 30;
		offset4 = offset0 + duty;
		break;
		//Traverse Gallop
	case 5:
		duty = 75;
		offset0 = 0;
		offset1 = 40;
		offset2 = 80;
		offset3 = 25;
		offset4 = offset2 + duty;
		break;
		//Bound
	case 6:
		duty = 70;
		offset0 = 30;
		offset1 = 65;
		offset2 = 30;
		offset3 = 65;
		offset4 = offset1 + duty;
		break;

	}
}

void Gait::setWalking(bool walking) {
	this->walking = walking;
}

void Gait::setWidth(double width) {
	this->width = width;
}

double Gait::getSpeed() const {
	return speed;
}

void Gait::setSpeed(double speed) {
	this->speed = speed;
	this->speedwait =1/(speed/length*offset4);
}

double Gait::getSpeedWait() const{
return speedwait;
}

void Gait::setSpeedWait(double sw){
this->speedwait=sw;
}

int Gait::getGaitmotion() const {
	return gaitmotion;
}

void Gait::setGaitmotion(int gaitmotion) {
	this->gaitmotion = gaitmotion;
}

